#include "gtest/gtest.h"

#include "predictor_acceleration.h"
#include "predictor_link.h"

using namespace std;

TEST( Predictors, acceleration )
{
  Predictor_base *p = new Predictor_const_a("TEST");

  p->get_input_link(0)->set_value(0.8); // acceleration
  p->get_input_link(1)->set_value(0.0); // velocity
  p->get_input_link(2)->set_value(0.0); // position

  p->save_state(0.0);
  p->predict(1.0);
  ASSERT_NEAR( 0.8, p->get_predicted_input(0), 0.000001 );
  ASSERT_NEAR( 0.8, p->get_predicted_input(1), 0.000001 );
  ASSERT_NEAR( 0.4, p->get_predicted_input(2), 0.000001 );

  p->predict(2.0);
  ASSERT_NEAR( 0.8, p->get_predicted_input(0), 0.000001 );
  ASSERT_NEAR( 1.6, p->get_predicted_input(1), 0.000001 );
  ASSERT_NEAR( 1.6, p->get_predicted_input(2), 0.000001 );

  p->predict(2.1);
  ASSERT_NEAR( 0.8, p->get_predicted_input(0), 0.000001 );
  ASSERT_NEAR( 1.68, p->get_predicted_input(1), 0.000001 );
  ASSERT_NEAR( 1.764, p->get_predicted_input(2), 0.000001 );
}
